////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////

#include "EnvelopeManager.hpp"

#ifdef __WXMAC__
#  ifdef __DARWIN__
#    include <OpenGL/glu.h>
#  else
#    include <glu.h>
#  endif
#else
#  include <GL/glu.h>
#endif

#include <cmath>
#include "Point3D.hpp"
#include "Vector3D.hpp"
#include "Matrix4D.hpp"
#include "MasksManager.hpp"

using namespace std;
using namespace Types;

////////////////////////////////////////////////////////////////////////////////
// Namespaces
////////////////////////////////////////////////////////////////////////////////

namespace SC {

////////////////////////////////////////////////////////////////////////////////
// Implementation
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Static data
////////////////////////////////////////////////////////////////////////////////

// Envelope material
GLfloat l_mat[4] = {1.f, 1.f, 1.f, 1.f};
// 3D vectors
Vector3D<float> s1, s2, u, v, n, mqR, g, v_normal;
// 3D points
Point3D<float> p1, p2, p3, p, sp, pt;
// World to local and Local to world coordinates transformation matrices
Matrix4D<float> wc2lc, lc2wc;
// Curve parameters
float ay, by, cy;
// Work variables
float x_step, ray, angle_step, s_ang_step, one_over_nb_stacks, one_over_half_nb_stacks, tmp1, tmp2;
// Colours
ColourRGBA col1, col2, col3;

#define MAX_NB_STACKS 32
#define MAX_NB_SLICES 32
#define NB_VERTICES_IDX (2*(MAX_NB_STACKS-1)*MAX_NB_SLICES+2)*3
#define NB_COLOURS_IDX (2*(MAX_NB_STACKS-1)*MAX_NB_SLICES+2)*4

// Vertices
GLfloat vertices[NB_VERTICES_IDX];
// Vertex pointer
GLfloat * vertex_ptr;

// Colours
GLfloat colours[NB_COLOURS_IDX];
// Colour pointer
GLfloat * colour_ptr;

// Normals
GLfloat normals[NB_VERTICES_IDX];
// Normal pointer
GLfloat * normal_ptr;

// Indices
GLuint indices[2*(MAX_NB_SLICES+2)+(2*MAX_NB_STACKS-2)*2*(MAX_NB_SLICES+1)];
// Index pointer
GLuint * index_ptr;

////////////////////////////////////////////////////////////////////////////////
// Public functions
////////////////////////////////////////////////////////////////////////////////

EnvelopeManager::EnvelopeManager()
:	nb_computations(0), nb_stacks(10), nb_slices(10)
{
	this->set_indices();
	s_ang_step = M_PI / (static_cast<float>(this->nb_stacks));
	one_over_nb_stacks = 1.f / (static_cast<float>(this->nb_stacks));
	one_over_half_nb_stacks = 1.f / (static_cast<float>(this->nb_stacks/2));
	angle_step = (2.f * M_PI) / (static_cast<float>(this->nb_slices));
}

void EnvelopeManager::Update(
	const std::pair< const Types::Point3D<float> *, const int * > sys1,
	const std::pair< const Types::Point3D<float> *, const int * > ctx,
	const std::pair< const Types::Point3D<float> *, const int * > sys2
) {
	int found = -1;
	for (int i=0; (found == -1) && (i<static_cast<int>(this->surfaces.size())); ++i)
		if ((this->surfaces[i].GetSecond().first == ctx.first) &&
			(	((this->surfaces[i].GetFirst().first == sys1.first) && (this->surfaces[i].GetThird().first == sys2.first))||
				((this->surfaces[i].GetFirst().first == sys2.first) && (this->surfaces[i].GetThird().first == sys1.first))	)	)
			found = i;
	if (found == -1)
		this->surfaces.push_back(
			Quadruple<
				const std::pair< const Types::Point3D<float> *, const int * >,
				const std::pair< const Types::Point3D<float> *, const int * >,
				const std::pair< const Types::Point3D<float> *, const int * >,
				unsigned long
			> (sys1, ctx, sys2, 1)
		);
	else ++this->surfaces[found].Fourth();
	++this->nb_computations;
}

void EnvelopeManager::SetNbStacks(const unsigned int value) {
	this->nb_stacks = (value>=MAX_NB_STACKS)?MAX_NB_STACKS:value;
	s_ang_step = M_PI/(static_cast<float>(this->nb_stacks));
	one_over_nb_stacks = 1.f/(static_cast<float>(this->nb_stacks));
	one_over_half_nb_stacks = 1.f / (static_cast<float>(this->nb_stacks/2));
	this->set_indices();
}

void EnvelopeManager::SetNbSlices(const unsigned int value) {
	this->nb_slices = (value>=MAX_NB_SLICES)?MAX_NB_SLICES:value;
	angle_step = (2.f * M_PI) / (static_cast<float>(this->nb_slices));
	this->set_indices();
}

#define DX 0.05f
void EnvelopeManager::DrawEnvelopes(
	const std::vector<int> & selected,
	const std::vector<MaskInfo> & masks,
	const float minimum,
	const float amplitude,
	const float opacity
) const {

	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, vertices);
	glEnableClientState(GL_COLOR_ARRAY);
	glColorPointer(4, GL_FLOAT, 0, colours);
	glEnableClientState(GL_NORMAL_ARRAY);
	glNormalPointer(GL_FLOAT, 0, normals);

	glEnable(GL_COLOR_MATERIAL);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, l_mat);

	for (unsigned int s=0; s<surfaces.size(); ++s) {

		// Hide envelope involving hidden systems
		bool show1 = true, show2 = true, show3 = true;
		if (!selected.empty()) {
			show1 = show2 = show3 = false;
			for (vector<int>::const_iterator itor = selected.begin(); (itor != selected.end()) && (!show1 || !show2 || !show3); ++itor) {
				if (*itor == *surfaces[s].GetFirst().second) show1 = true;
				if (*itor == *surfaces[s].GetSecond().second) show2 = true;
				if (*itor == *surfaces[s].GetThird().second) show3 = true;
			}
		}

		// Get systems type index
		col1 = masks[*surfaces[s].GetFirst().second].colour;	if (!show1) col1.a = 0.f; else col1.a *= opacity;
		col2 = masks[*surfaces[s].GetSecond().second].colour;	if (!show2) col2.a = 0.f; else col2.a *= opacity;
		col3 = masks[*surfaces[s].GetThird().second].colour;	if (!show3) col3.a = 0.f; else col3.a *= opacity;

		// Get points
		p1 = *surfaces[s].GetFirst().first;
		p2 = *surfaces[s].GetSecond().first;
		p3 = *surfaces[s].GetThird().first;

		// Get interaction vectors
		s1 = p1 - p2;
		s2 = p3 - p2;

		// Create new basis
		n = (s2 * s1).GetNormalised();
		v = (s1 + s2).GetNormalised();
		u = v * n;

		// Create transformation matrices
		mqR.x = - ( p2.x*u.x + p2.y*u.y + p2.z*u.z );
		mqR.y = - ( p2.x*v.x + p2.y*v.y + p2.z*v.z );
		mqR.z = - ( p2.x*n.x + p2.y*n.y + p2.z*n.z );

		wc2lc(0,0) = u.x;		wc2lc(0,1) = v.x;		wc2lc(0,2) = n.x;		wc2lc(0,3) = 0.f;
		wc2lc(1,0) = u.y;		wc2lc(1,1) = v.y;		wc2lc(1,2) = n.y;		wc2lc(1,3) = 0.f;
		wc2lc(2,0) = u.z;		wc2lc(2,1) = v.z;		wc2lc(2,2) = n.z;		wc2lc(2,3) = 0.f;
		wc2lc(3,0) = mqR.x;		wc2lc(3,1) = mqR.y;		wc2lc(3,2) = mqR.z;		wc2lc(3,3) = 1.f;

		lc2wc(0,0) = u.x;		lc2wc(0,1) = u.y;		lc2wc(0,2) = u.z;		lc2wc(0,3) = 0.f;
		lc2wc(1,0) = v.x;		lc2wc(1,1) = v.y;		lc2wc(1,2) = v.z;		lc2wc(1,3) = 0.f;
		lc2wc(2,0) = n.x;		lc2wc(2,1) = n.y;		lc2wc(2,2) = n.z;		lc2wc(2,3) = 0.f;
		lc2wc(3,0) = p2.x;		lc2wc(3,1) = p2.y;		lc2wc(3,2) = p2.z;		lc2wc(3,3) = 1.f;

		// Get points in lc basis
		p1 = wc2lc.MultLeft3D(p1);
		p2 = wc2lc.MultLeft3D(p2);
		p3 = wc2lc.MultLeft3D(p3);

		// Find curve in lc basis
		ay = ( p1.x*(p2.y-p3.y)+p2.x*(p3.y-p1.y)+p3.x*(p1.y-p2.y) ) / (p1.x*(p2.x*p2.x-p3.x*p3.x)+p2.x*(p3.x*p3.x-p1.x*p1.x)+p3.x*(p1.x*p1.x-p2.x*p2.x));
		by = ((p3.y-p1.y) - ay*(p3.x*p3.x-p1.x*p1.x)) / (p3.x-p1.x);
		cy = p3.y - p3.x*(ay*p3.x + by);

		// === Find envelope vertices along the interpolation points ===
		vertex_ptr = vertices;
		colour_ptr = colours;
		normal_ptr = normals;

		// Envelope ray
		ray = minimum + amplitude * ((static_cast<float>(surfaces[s].GetFourth())) / (static_cast<float>(this->nb_computations)));

		// --- First half-sphere ---
		{

			// Get gradient vector in lc
			g.x = DX;
			g.y = (2.f*ay*p1.x + by) * DX;
			g.z = 0.f;
			g.Normalise();

			// Translate gradient vector to wc
			v = lc2wc.TranslateVectorLeft3D(g);

			// Resize gradient to ray length
			g *= ray;

			// Get sphere top point in lc
			sp = p1 - g;

			// Translate top point to wc and add the vertex
			p = lc2wc.MultLeft3D(sp + g * one_over_half_nb_stacks * 0.5f);
			*vertex_ptr++ = p.x;	*vertex_ptr++ = p.y;	*vertex_ptr++ = p.z;
			*colour_ptr++ = col1.r;	*colour_ptr++ = col1.g;	*colour_ptr++ = col1.b; *colour_ptr++ = col1.a;
			*normal_ptr++ = -v.x;	*normal_ptr++ = -v.y;	*normal_ptr++ = -v.z;

			// Get circular vector
			u = n * v;

			// Trace half-sphere with half the precision of the tube
			for (float t=one_over_half_nb_stacks, s_ang=M_PI*0.5f-s_ang_step; t<1.f; t+=one_over_half_nb_stacks, s_ang-=s_ang_step) {

				// Get new centre point
				p.x = sp.x + g.x*t;
				p.y = sp.y + g.y*t;
				p.z = 0.f;

				// Translate centre point to wc
				p = lc2wc.MultLeft3D(p);

				// Get current ray and compute circular points
				for (float angle=0.f; angle<2.f*M_PI; angle+=angle_step) {
					v_normal = n*cos(angle) + u*sin(angle);
					pt = p + v_normal * ray * cos(s_ang);
					*vertex_ptr++ = pt.x;		*vertex_ptr++ = pt.y;		*vertex_ptr++ = pt.z;
					*colour_ptr++ = col1.r;		*colour_ptr++ = col1.g;		*colour_ptr++ = col1.b;  	*colour_ptr++ = col1.a;
					*normal_ptr++ = v_normal.x;	*normal_ptr++ = v_normal.y;	*normal_ptr++ = v_normal.z;
				}

			}

		}

		// --- Pipe ---
		tmp1 = 1.f / (p2.x-p1.x);
		tmp2 = 1.f / (p3.x-p2.x);
		x_step = (p3.x - p1.x) * one_over_nb_stacks;
		for (float x=p1.x; x<p3.x+x_step*0.5f; x+=x_step) {

			// Get point in lc basis
			p.x = x;
			p.y = x*(x*ay+by)+cy;
			p.z = 0.f;

			// Translate centre point to wc
			p = lc2wc.MultLeft3D(p);

			// Get gradient vector in lc
			g.x = 0.01f;
			g.y = (2.f*ay*x + by) * 0.01f;
			g.z = 0.f;

			// Translate gradient vector to wc
			v = lc2wc.TranslateVectorLeft3D(g);
			v.Normalise();

			// Get circular vector
			u = n * v;

			// Compute circular points
			for (float angle=0.f; angle<2.f*M_PI; angle+=angle_step) {
				v_normal = n*cos(angle) + u*sin(angle);
				pt = p + v_normal * ray;
				*vertex_ptr++ = pt.x;	*vertex_ptr++ = pt.y;	*vertex_ptr++ = pt.z;
				if (x <= p2.x) {
					*colour_ptr++ = col1.r + (x-p1.x)*tmp1*(col2.r-col1.r);
					*colour_ptr++ = col1.g + (x-p1.x)*tmp1*(col2.g-col1.g);
					*colour_ptr++ = col1.b + (x-p1.x)*tmp1*(col2.b-col1.b);
					*colour_ptr++ = col1.a + (x-p1.x)*tmp1*(col2.a-col1.a);
				}
				else {
					*colour_ptr++ = col2.r + (x-p2.x)*tmp2*(col3.r-col2.r);
					*colour_ptr++ = col2.g + (x-p2.x)*tmp2*(col3.g-col2.g);
					*colour_ptr++ = col2.b + (x-p2.x)*tmp2*(col3.b-col2.b);
					*colour_ptr++ = col2.a + (x-p2.x)*tmp2*(col3.a-col2.a);
				}
				*normal_ptr++ = v_normal.x;	*normal_ptr++ = v_normal.y;	*normal_ptr++ = v_normal.z;
			}

		}

		// --- Second half-sphere ---
		{

			// Get gradient vector in lc
			g.x = DX;
			g.y = (2.f*ay*p3.x + by) * DX;
			g.z = 0.f;
			g.Normalise();

			// Translate gradient vector
			v = lc2wc.TranslateVectorLeft3D(g);

			// Resize gradient to ray length
			g *= ray;

			// Get sphere top point in lc
			sp = p3 + g;

			// Get circular vector
			u = n * v;

			// Trace half-sphere with half the precision of the tube
			for (float t=one_over_half_nb_stacks, s_ang=s_ang_step; t<1.f; t+=one_over_half_nb_stacks, s_ang+=s_ang_step) {

				// Get new centre point
				p.x = p3.x + g.x*t;
				p.y = p3.y + g.y*t;
				p.z = 0.f;

				// Translate centre point to wc
				p = lc2wc.MultLeft3D(p);

				// Get current ray and compute circular points
				for (float angle=0.f; angle<2.f*M_PI; angle+=angle_step) {
					v_normal = n*cos(angle) + u*sin(angle);
					pt = p + v_normal * ray * cos(s_ang);
					*vertex_ptr++ = pt.x;		*vertex_ptr++ = pt.y;		*vertex_ptr++ = pt.z;
					*colour_ptr++ = col3.r;		*colour_ptr++ = col3.g;		*colour_ptr++ = col3.b;		*colour_ptr++ = col3.a;
					*normal_ptr++ = v_normal.x;	*normal_ptr++ = v_normal.y;	*normal_ptr++ = v_normal.z;
				}
			}

			// Translate top point to wc and add the vertex
			p = lc2wc.MultLeft3D(sp - g * one_over_half_nb_stacks * 0.5f);
			*vertex_ptr++ = p.x;	*vertex_ptr++ = p.y;	*vertex_ptr++ = p.z;
			*colour_ptr++ = col3.r;	*colour_ptr++ = col3.g;	*colour_ptr++ = col3.b;	*colour_ptr++ = col3.a;
			*normal_ptr++ = v.x;	*normal_ptr++ = v.y;	*normal_ptr++ = v.z;
		}

		// Draw it
		index_ptr = indices;
		glDrawElements(GL_TRIANGLE_FAN, this->nb_slices+2, GL_UNSIGNED_INT, index_ptr);

		index_ptr += this->nb_slices+2;
		for (unsigned int i=0; i<2*this->nb_stacks-2; ++i) {
			glDrawElements(GL_QUAD_STRIP, 2*(this->nb_slices+1), GL_UNSIGNED_INT, index_ptr);
			index_ptr += 2*(this->nb_slices+1);
		}
		glDrawElements(GL_TRIANGLE_FAN, this->nb_slices+2, GL_UNSIGNED_INT, index_ptr);

	} // surfaces

	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);
	glDisable(GL_COLOR_MATERIAL);

}

////////////////////////////////////////////////////////////////////////////////
// Protected functions
////////////////////////////////////////////////////////////////////////////////

void EnvelopeManager::set_indices() {
	index_ptr = indices;

	*index_ptr++ = 0;
	for (unsigned int j=0; j<this->nb_slices; ++j)
		*index_ptr++ = j+1;
	*index_ptr++ = 1;

	for (unsigned int i=0; i<2*this->nb_stacks-2; ++i) {
		unsigned int start = 1 + i*this->nb_slices;
		for (unsigned int j=0; j<nb_slices; ++j) {
			*index_ptr++ = start+j;
			*index_ptr++ = start+j + this->nb_slices;
		}
		*index_ptr++ = start;
		*index_ptr++ = start + this->nb_slices;
	}

	unsigned int start = 1 + (2*this->nb_stacks-2)*this->nb_slices;
	*index_ptr++ = start + this->nb_slices;
	for (unsigned int j=0; j<this->nb_slices; ++j)
		*index_ptr++ = start+(this->nb_slices-1)-j;
	*index_ptr++ = start+(this->nb_slices-1);
}

////////////////////////////////////////////////////////////////////////////////
// Closing Namespaces
////////////////////////////////////////////////////////////////////////////////

}

////////////////////////////////////////////////////////////////////////////////
